/* -------------------------------------------------------------------------------
----------------------------------------------------------------------------------
    Jacobian library for VxWorks on cRio
    to replace Robotics module G-code. Based
    on Peter Coark's work.
----------------------------------------------------------------------------------
-------------------------------------------------------------------------------- */
#include <math.h>
#include <string.h>
#define M_PI 3.14159265358979323846

    const double alpha_ij [] = {M_PI/2, 0.0, M_PI/2, M_PI/2, M_PI/2, 0.0};
    const double a_ij [] = {0.0, 0.45, 0.0, 0.0, 0.0, 0.0};
    const double S_ij [] = {0.0, 0.0, 0.0, 0.48, 0.0, 0.07};
    const double offset [] = {M_PI/2, M_PI/2, M_PI/2, M_PI, M_PI, 0.0};


extern "C" int SpeedTest() {
    return 0;
}

extern "C" int Jacobian(const double* q, double JC[][6])
{
    double sum;
    int ii,n,ll;
    double a;
    double alpha;
    double thetaA;
    double S;
    /* ---------------------------------------
    Remember there is a difference between initialization and assigning
    arrays in C++. You can init. arrays this way but NOT assign. Since this
    method is so convenient, declare a new T_x array each time. Better solution
    is to use vectors, newmat, classes, etc....
    ----------------------------------------- */
    double U[4][4] = { {1.0, 0.0, 0.0, 0.0},
                       {0.0, 1.0, 0.0, 0.0},
                       {0.0, 0.0, 1.0, 0.0},
                       {0.0, 0.0, 0.0, 1.0} };
    double Ures[4][4];

/* ----<>---------------------------------
    NOTE: This method uses the alternative to Crane's method for forming
    Transformations between this links. You would think this would cause problems
    but this way returns the correct Jacobian. Maybe it has something do with the order
    processed?
        Loop unrolling for Joint 6
------------------------------------------*/
        int jj = 6;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_1[4][4] =  {   {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                                {0.0, sin(alpha), cos(alpha), S},
                                {0.0, 0.0, 0.0, 1.0} };

        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_1[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));

        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

/* ----<>---------------------------------
        Loop unrolling for Joint 5
------------------------------------------*/
        jj = 5;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_2[4][4] =   {  {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                                {0.0, sin(alpha), cos(alpha), S},
                                {0.0, 0.0, 0.0, 1.0} };
        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_2[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));

        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

/* ----<>---------------------------------
        Loop unrolling for Joint 4
------------------------------------------*/
        jj = 4;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_3[4][4] =   {  {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                                {0.0, sin(alpha), cos(alpha), S},
                                {0.0, 0.0, 0.0, 1.0} };
        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_3[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));


        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

/* ----<>---------------------------------
        Loop unrolling for Joint 3
------------------------------------------*/
        jj = 3;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_4[4][4] =   {  {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                                {0.0, sin(alpha), cos(alpha), S},
                                {0.0, 0.0, 0.0, 1.0} };
        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_4[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));


        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

/* ----<>---------------------------------
        Loop unrolling for Joint 2
------------------------------------------*/
        jj = 2;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_5[4][4] =   {  {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                                {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                                {0.0, sin(alpha), cos(alpha), S},
                                {0.0, 0.0, 0.0, 1.0} };
        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_5[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));

        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

/* ----<>---------------------------------
        Loop unrolling for Joint 1
------------------------------------------*/
        jj = 1;
        a = a_ij[jj-1];
        alpha = alpha_ij[jj-1];
        thetaA = q[jj-1]+offset[jj-1];
        S = S_ij[jj-1];
        double T_6[4][4] =  {  {cos(thetaA), -sin(thetaA)*cos(alpha), sin(thetaA)*sin(alpha), a*cos(thetaA) },
                               {sin(thetaA), cos(thetaA)*cos(alpha), -cos(thetaA)*sin(alpha), a*sin(thetaA) },
                               {0.0, sin(alpha), cos(alpha), S},
                               {0.0, 0.0, 0.0, 1.0} };
        for (ii=0; ii<4; ii++) {
            for (n=0; n<4; n++) {
                sum = 0;
                for (ll=0; ll<4; ll++) {
                    sum += T_6[ii][ll]*U[ll][n];
                }
                Ures[ii][n] = sum;
            }
        }
        memcpy(U, Ures, sizeof(Ures));

        JC[0][jj-1] = -U[0][0]*U[1][3]+U[1][0]*U[0][3];
        JC[1][jj-1] = -U[0][1]*U[1][3]+U[1][1]*U[0][3];
        JC[2][jj-1] = -U[0][2]*U[1][3]+U[1][2]*U[0][3];
        JC[3][jj-1] = U[2][0];
        JC[4][jj-1] = U[2][1];
        JC[5][jj-1] = U[2][2];

    return 0;
}


        /* -----<>-----------------------------------------------------------------------------------------

        T.row(1) <<  cos(thetaA) << -sin(thetaA)*cos(alpha)    <<  sin(thetaA)*sin(alpha)  << a*cos(thetaA);
        T.row(2) <<  sin(thetaA) << cos(thetaA)*cos(alpha)     <<  -cos(thetaA)*sin(alpha) << a*sin(thetaA);
        T.row(3) <<  0          << sin(alpha)                <<  cos(alpha)             << S;
        T.row(4) <<  0          << 0 <<  0 << 1;
        U = T*U;
        JC[0][jj-1] = -U(1,1)*U(2,4)+U(2,1)*U(1,4);
        JC[1][jj-1] = -U(1,2)*U(2,4)+U(2,2)*U(1,4);
        JC[2][jj-1] = -U(1,3)*U(2,4)+U(2,3)*U(1,4);
        JC[3][jj-1] = U(3,1);
        JC[4][jj-1] = U(3,2);
        JC[5][jj-1] = U(3,3); */
